﻿﻿
using System;
using System.Threading;
using System.Collections.Generic;
using Newtonsoft.Json;      //引用命名空间 [dotnet add package Newtonsoft.Json]
using System.Linq;

namespace _Car2dEnv
{
    public class MyVector2 : BdUtils.IJson
    {
        public int x { get { return this.arr[0]; } set { arr[0] = value; } }
        public int y { get { return this.arr[1]; } set { arr[1] = value; } }

        public int[] arr = null;

        public MyVector2(int x, int y)
        {
            this.arr = new int[2] { -1, -1 };
            this.arr[0] = x;
            this.arr[1] = y;
        }
        public MyVector2(object desination_pos)
        {
            if (desination_pos == null)
                return;
            var _desination_pos = BdUtils.conv_obj_to_list(desination_pos);
            int x = Convert.ToInt32(_desination_pos[0]);
            int y = Convert.ToInt32(_desination_pos[1]);
            this.arr = new int[2] { -1, -1 };
            this.arr[0] = x;
            this.arr[1] = y;
        }

        //重载一个下标运算符号
        public int this[int index]
        {
            get
            {
                return arr[index];
            }
            set
            {
                arr[index] = value;
            }
        }

        public static bool operator ==(MyVector2 v1, MyVector2 v2)
        {
            return is_equal(v1, v2);
        }

        public static bool operator !=(MyVector2 v1, MyVector2 v2)
        {
            return !is_equal(v1, v2);
        }

        public static MyVector2 operator -(MyVector2 v1, MyVector2 v2)
        {
            int x = v1.x - v2.x;
            int y = v1.y - v2.y;
            MyVector2 res = new MyVector2(x, y);

            return res;
        }

        public double get_magnitude()
        {
            double _magnitude = (double)Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2));
            return _magnitude;
            // get
            // {
            //     double _magnitude = (double)Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2));
            //     return _magnitude;
            // }
        }

        public override bool Equals(object obj)
        {
            if (obj == null)
                return false;
            if (GetType() != obj.GetType())
                return false;
            MyVector2 rec = (MyVector2)obj;
            return is_equal(this, rec);
        }

        static bool is_equal(MyVector2 v1, MyVector2 v2)
        {
            bool ret = true;
            if (v1.x != v2.x || v1.y != v2.y)
            {
                ret = false;
            }
            return ret;
        }

        public override int GetHashCode()
        {
            //在这里使用字符串数组的hashcode，避免自己完成一个算法
            return arr.GetHashCode();
        }
    }

    public class InitialState : BdUtils.IJson
    {
        public MyVector2 agent_pos;
        public MyVector2 desination_pos;

        public InitialState(
            object agent_pos = null,
            object desination_pos = null,
            object screen_size = null,
            int time_interval = 500,
            string info = ""
        )
        {
            if (screen_size != null)
            {
                var _screen_size = BdUtils.conv_obj_to_list(screen_size);
                this.screen_x = Convert.ToInt32(_screen_size[0]);
                this.screen_y = Convert.ToInt32(_screen_size[1]);


                this.agent_x = this.agent_x < screen_x ? agent_x : BdUtils.TimeUtils.get_random_number(1, screen_x);

                // BdUtils.log("--- agent_x: ", agent_x, "--- screen_x: ", screen_x, " --- flag:", agent_x < screen_x);

                this.agent_y = this.agent_y < screen_y ? agent_y : BdUtils.TimeUtils.get_random_number(1, screen_y);
                this.desination_x = this.desination_x < screen_x ? desination_x : BdUtils.TimeUtils.get_random_number(1, screen_x);
                this.desination_y = this.desination_y < screen_y ? desination_y : BdUtils.TimeUtils.get_random_number(1, screen_y);

                // BdUtils.log("--- InitialState: ", this);
                // BdUtils.TimeUtils.sleep(3);
            }


            if (agent_pos != null)
            {
                var _agent_pos = BdUtils.conv_obj_to_list(agent_pos);
                this.agent_x = Convert.ToInt32(_agent_pos[0]);
                this.agent_y = Convert.ToInt32(_agent_pos[1]);
            }
            this.agent_pos = new MyVector2(this.agent_x, this.agent_y);

            if (desination_pos != null)
            {
                var _desination_pos = BdUtils.conv_obj_to_list(desination_pos);
                this.desination_x = Convert.ToInt32(_desination_pos[0]);
                this.desination_y = Convert.ToInt32(_desination_pos[1]);
            }
            this.desination_pos = new MyVector2(this.desination_x, this.desination_y);

            this.time_interval = time_interval;

            if (info != "")
            {
                this.info = info;
            }
        }
        public int time_interval { get; set; } = 500;
        public int agent_x = 10;   //石头x初始值
        public int agent_y = 1;    //石头y初始值

        public int desination_x = 15;
        public int desination_y = 15;
        public int screen_x = 20;
        public int screen_y = 20;
        public bool done = false;
        public float reward = .0f;
        public string info = "";

    }

    public class Car2dEnv : BdUtils.IJson
    {
        public bool stop_flag = false;

        public int _i_action = 0;
        public int i_action = 0;

        public InitialState init_state;
        public InitialState current_state = null;

        public System.Random rd;
        int? seed;

        string env_id;
        string play_model;

        public List<string> PLAY_MODEL_VALUE_RANGE = new List<string> { "human", "sample", "net" };

        public int? time_interval = null;

        public int size;
        public int max_run_times_per_round = 1000;        // 每回合最大运行次数

        public Car2dEnv(string env_id = null, int? seed = null, int size = 10, string play_model = "human", int max_run_times_per_round = 0, int? time_interval = null)
        {
            this.seed = seed;

            if (seed == null)
            {
                this.rd = new System.Random(Guid.NewGuid().GetHashCode());     // 解决并发问题
            }
            else
            {
                this.rd = new System.Random(Convert.ToInt32(seed));
            }

            if (env_id == null)
            {
                this.env_id = BdUtils.TimeUtils.get_time_int();
            }

            this.size = size;

            this.s2i_action_dc = i2s_action_dc.ToDictionary(k => k.Value, p => p.Key);

            BdUtils.assert(PLAY_MODEL_VALUE_RANGE.Contains(play_model), $"play_model[{play_model}]取值错误: play_model取值必须在{BdUtils._log(PLAY_MODEL_VALUE_RANGE)}中!");
            this.play_model = play_model;

            if (max_run_times_per_round > 0) this.max_run_times_per_round = max_run_times_per_round;

            this.time_interval = time_interval;

            this.observation_space = new List<List<int>> {
                new List<int> { 0, 0, 0, 0 },      // low
                new List<int> { size, size, size, size }       // high
            };

            this.action_space_n = this.action_space.Count();

            // this.init_state = new InitialState(screen_size: new int[] { size, size });
        }

        // observation_space: [agent_x, agent_y, destination_x, destination_y]
        public List<List<int>> observation_space = new List<List<int>>();       // 观测值[x, y]的取值范围
        public int[] observation_space_shape = new int[] { 1, 4 };

        public List<int> action_space = new List<int> { 0, 1, 2, 3, 4 };

        public int action_space_n;

        public int choose_action()
        {
            int action = 0;
            if (this.play_model == "human")
            {
                action = this.get_an_action_by_human_input();
            }
            else if (this.play_model == "sample")
            {
                action = get_an_action_by_sample();
            }
            else if (this.play_model == "net")
            {
                action = get_an_action_from_net_commands();
                if (!action_space.Contains(action))
                {
                    BdUtils.log($"*** Error: action[{action}]取值错误!");
                    action = 0;
                }
            }
            else
            {
                BdUtils.log($"取值错误, play_model: {play_model}");
            }

            _i_action += 1;  // choose_action
            return action;
        }
        public int get_an_action_by_human_input()
        {
            char c;
            int action = 0;
            if (Console.KeyAvailable)
            {
                ConsoleKeyInfo x = Console.ReadKey();
                c = x.KeyChar;
                if (this.c2s_action_dc.ContainsKey(c))
                    action = this.s2i_action_dc[this.c2s_action_dc[c]];
            }
            return action;

        }
        public int get_an_action_by_sample()
        {
            int action = this.rd.Next(this.action_space_n);
            // BdUtils.log($"--- action --- {action}");
            return action;
        }

        // List<int> net_commands = new List<int> { 1, 0, 0, 1, 1, 2, 2, 0, 0, 3, 3, 4, 4 };
        // Queue<int> net_commands = new Queue<int> { 1, 0, 0, 1, 1, 2, 2, 0, 0, 3, 3, 4, 4 };
        // public Queue<int> net_commands = new Queue<int> { new int[] { 1, 2, 3, 4 } };
        // public Queue<int> _net_commands = new Queue<int>(new int[] { 0, 0, 0, 1, 1, 2, 2, 0, 0, 3, 3, 4, 4 });
        // public Queue<int> net_commands = new Queue<int>(new int[] { 1, 0, 0, 1, 1, 2, 2, 0, 0, 3, 3, 4, 4 });
        public Queue<int> net_commands = new Queue<int>();

        public int get_an_action_from_net_commands()
        {
            int action = 0;
            lock (net_commands)
                if (net_commands.Count != 0)
                {
                    action = net_commands.Dequeue();
                    // BdUtils.log($"--- net_commands action --- {action}", BdUtils.end_string);
                    // BdUtils.TimeUtils.sleep(0.2);
                }
            return action;
        }

        public Dictionary<int, string> i2s_action_dc = new Dictionary<int, string> {
            {0, "stop"},
            {1, "up"},
            {2, "down"},
            {3, "left"},
            {4, "right"},
        };

        public Dictionary<string, int> s2i_action_dc;  // convert actrion from string_to_int(s2i)

        public Dictionary<char, string> c2s_action_dc = new Dictionary<char, string> {
            {'t', "stop"},
            {'w', "up"},
            {'s', "down"},
            {'a', "left"},
            {'d', "right"},
        };

        public MyVector2 get_random_vector2()
        {
            // size为20时, 范围为[1, 18], 因为等于0和19的时候都是墙壁!
            int size = this.size;
            int x = this.rd.Next(1, size - 1);
            int y = this.rd.Next(1, size - 1);
            return new MyVector2(x, y);
        }

        public int current_epoch = 0;
        public MyStateRet reset(bool clear_console = true)
        {
            current_epoch += 1;

            if (clear_console)
                Console.Clear();
            this.run_times = 0;

            // MyVector2 agent_pos = new MyVector2(10, 10);
            MyVector2 agent_pos = get_random_vector2();

            MyVector2 desination_pos;
            do
            {
                desination_pos = get_random_vector2();
            } while (desination_pos == agent_pos);

            this.init_state = new InitialState(
                agent_pos: agent_pos.arr,
                desination_pos: desination_pos.arr,
                screen_size: new int[] { this.size, this.size },
                info: size.ToString()
            );
            this.current_state = this.init_state;

            var ret = get_current_state();

            // msg reset
            if (this.play_model == "net")
            {
                string info = $"[reset] --- current_epoch[{current_epoch}]]";
                var my_msg = new MsgState(ret, info: info, i: i_action);
                NetManager.Send(my_msg);  // reset

                // var my_msg = new MsgState(ret, "reset");
                // NetManager.Send(my_msg);
            }
            // i_action += 1;
            return ret;
        }

        public int run_times = 0;

        public MyStateRet step(object action)
        {
            run_times += 1;

            string action_string;
            if (BdUtils.is_integer(action))
                action_string = i2s_action_dc[Convert.ToInt32(action)];
            else
                action_string = action.ToString();

            switch (action_string)
            {
                case "stop":
                    break;
                case "up":
                    this.current_state.agent_pos.y -= 1;
                    break;
                case "down":
                    this.current_state.agent_pos.y += 1;
                    break;
                case "left":
                    this.current_state.agent_pos.x -= 1;
                    break;
                case "right":
                    this.current_state.agent_pos.x += 1;
                    break;
                default:
                    BdUtils.log("! --- default! action_string: ", action_string);
                    break;
            }

            bool done = false;

            // 获取reward
            MyVector2 tmp = this.current_state.agent_pos - this.current_state.desination_pos;
            MyVector2 max_magnitude_vector2 = new MyVector2(this.init_state.screen_x, this.init_state.screen_y);

            // BdUtils.log("magnitude tmp: ", Math.Round(tmp.get_magnitude(), 2), "magnitude max: ", Math.Round(max_magnitude_vector2.get_magnitude(), 2), BdUtils.end_string);

            double rate_for_reward_by_distance = tmp.get_magnitude() / max_magnitude_vector2.get_magnitude();

            // float reward = (float)Math.Round(-0.01 * rate_for_reward_by_distance, 4);     // 默认扣分, 避免瞎逛
            float reward = (float)Math.Round(-1.0 / 2 / this.size * rate_for_reward_by_distance, 4);     // 默认扣分, 避免瞎逛
            reward = (float)Math.Min(reward, -0.005f);

            // float reward = -0.01f;     // 默认扣分, 避免瞎逛

            BdUtils.log("--- reward:", reward, "--- action: ", action, BdUtils.end_string);

            BdUtils.log("--------------- reward:", reward.ToString("0.0000"), $"--- action: {action} --- agent_pos: ", this.current_state.agent_pos, ", desination_pos:", this.current_state.desination_pos, BdUtils.end_string);

            // 逛太久了, 直接结束开始下一回合
            if (run_times >= this.max_run_times_per_round)
            {
                done = true;
            }

            // 撞到墙壁 or 超过边界, 则结束
            if ((this.current_state.agent_pos.x <= 0 || this.current_state.agent_pos.x >= size - 1) || (this.current_state.agent_pos.y <= 0 || this.current_state.agent_pos.y >= size - 1))
            {
                // BdUtils.log("撞到墙壁 or 超过边界, this.current_state:", this.current_state);
                // BdUtils.TimeUtils.sleep(3);
                done = true;
                reward = -1f;
            }

            // 到达目标点也结束
            if (this.current_state.agent_pos == this.current_state.desination_pos)
            {
                // BdUtils.log("到达目标点也结束");
                // BdUtils.TimeUtils.sleep(3);

                done = true;
                reward = 1f;
            }

            this.current_state.done = done;
            this.current_state.reward = reward;

            string info = size.ToString();
            this.current_state.info = info;

            var ret = get_current_state();

            // msg step
            if (this.play_model == "net")
            {
                info = $"current_epoch[{current_epoch}], i_action[{i_action}]";
                var my_msg = new MsgState(ret, info: info, i: i_action);
                NetManager.Send(my_msg);
            }
            i_action += 1; // step
            return ret;
        }

        public MyStateRet get_current_state()
        {
            MyStateRet ret = null;
            EnvObservation observation = new EnvObservation(this.current_state.agent_pos, this.current_state.desination_pos);

            ret = new MyStateRet(
                observation, this.current_state.reward, this.current_state.done, this.current_state.info
            );
            return ret;
        }

        public class EnvObservation : BdUtils.IJson
        {
            public EnvObservation(MyVector2 agent_pos, MyVector2 destination_pos)
            {
                this.agent_pos = agent_pos.arr;
                this.destination_pos = destination_pos.arr;
            }
            // public MyVector2 agent_pos;
            public int[] agent_pos = null;
            // public MyVector2 destination_pos;
            public int[] destination_pos = null;

        }

        public class MyStateRet : BdUtils.IJson
        {
            public EnvObservation obsevation;
            public float reward;
            public bool done;
            public string info;

            public MyStateRet(EnvObservation observation, float reward, bool done, string info)
            {
                this.obsevation = observation;
                this.reward = reward;
                this.done = done;
                this.info = info;
            }
        }

        public void render()
        {
            // int time_interval = 0;
            int time_interval;
            if (this.time_interval == null)
                time_interval = this.current_state.time_interval;
            else
                time_interval = Convert.ToInt32(this.time_interval);

            int agent_x = this.current_state.agent_pos.x;
            int agent_y = this.current_state.agent_pos.y;

            int desination_x = this.current_state.desination_pos.x;
            int desination_y = this.current_state.desination_pos.y;

            int screen_x = this.current_state.screen_x;
            int screen_y = this.current_state.screen_y;

            string current_view = "";

            current_view = "";

            for (int y = 0; y < screen_y; y++)
            {
                for (int x = 0; x < screen_x; x++)
                {
                    // agent到达目标点
                    if (x == agent_x && y == agent_y && x == desination_x && y == desination_y)
                    {
                        current_view += "+";
                        continue;
                    }

                    // 自身位置
                    if (x == agent_x && y == agent_y)
                    {
                        if (this.current_state.done)
                            current_view += "X";
                        else
                            current_view += "A";
                        continue;
                    }

                    // 目标点
                    if (x == desination_x && y == desination_y)
                    {
                        current_view += "D";
                        continue;
                    }

                    // 围墙
                    if (y == 0 || y == screen_y - 1)
                    {
                        current_view += "-";
                        continue;
                    }

                    if (x == 0 || x == screen_x - 1)
                    {
                        current_view += "|";
                        continue;
                    }

                    current_view += " ";
                }
                current_view += BdUtils.end_string + "\n";
            }

            Console.SetCursorPosition(0, 0);
            Console.WriteLine(current_view);

            if (time_interval != 0)
                System.Threading.Thread.Sleep(time_interval);   //停顿0.5秒
        }

        public void _render()
        {
            if (play_model == "net")
                if (!NetManager.is_connected())
                    return;
            // int time_interval = this.current_state.time_interval;
            int time_interval;
            if (this.time_interval == null)
                time_interval = this.current_state.time_interval;
            else
                time_interval = Convert.ToInt32(this.time_interval);

            int agent_x = this.current_state.agent_x;
            int agent_y = this.current_state.agent_y;

            int desination_x = this.current_state.desination_x;
            int desination_y = this.current_state.desination_y;

            int screen_x = this.current_state.screen_x;
            int screen_y = this.current_state.screen_y;

            string current_view = "";
            int run_times = 0;
            int MAX_RUNTIMES = 10000;

            Console.Clear();
            while (run_times < MAX_RUNTIMES)
            {
                run_times++;

                current_view = "";

                current_view += this.init_state.ToString() + '\n';

                for (int y = 0; y < screen_y; y++)
                {
                    for (int x = 0; x < screen_x; x++)
                    {
                        // 自身位置
                        if (x == agent_x && y == agent_y)
                        {
                            current_view += "0";
                            continue;
                        }


                        // 目标点
                        if (x == desination_x && y == desination_y)
                        {
                            current_view += "T";
                            continue;
                        }

                        // 围墙
                        if (y == 0 || y == screen_y - 1)
                        {
                            current_view += "-";
                            continue;
                        }
                        if (x == 0 || x == screen_x - 1)
                        {
                            current_view += "|";
                            continue;
                        }

                        current_view += " ";
                    }
                    current_view += "\n";
                }

                agent_y++;   //石头y值+1

                Console.SetCursorPosition(0, 0);
                Console.WriteLine(current_view);

                if (agent_y >= screen_y)
                {
                    break;
                }

                if (time_interval != 0) System.Threading.Thread.Sleep(time_interval);   //停顿0.5秒
            }
            // Console.WriteLine("结束.");
        }

    }

}